
--module(..., package.seeall)

--------------------------------------------------------------
--    使用方法:
--    1. Motoe.Run = 电机运行时间(ms), 正数正转, 负数反转
--    2. 正反转直接有3秒停顿时间(GapTick =3000ms)
--------------------------------------------------------------



local sys = require 'sys'
-- 加载I2C端口扩展芯片,再加载"Led_Key_IO_etc"文件，驱动的LED,键盘,蜂鸣器,电机等
    --require "MCP23017"  
        require "Led_Key_IO_etc" 


--------------  电机驱动信号  ----------------------------
-- 函数： 电机正转/反转/停机, 驱动定义在"Led_Key_IO_etc.lua"
local function MOTOR_CW()    TurnOff_CCW()  TurnOn_CW()     end
local function MOTOR_CCW()   TurnOff_CW()   TurnOn_CCW()    end
local function MOTOR_STOP()  TurnOff_CW()   TurnOff_CCW()   end


-------------- 电机参数表, 控制周期50ms  ------------------
Motor = {   
    Run     = 0,            -- 设置电机运行时间:  >0,电机正转; <0:反转;0:停止; 时间单位ms

    Status  = 'CW_STOP',     -- 当前电机的状态,有 CCW、CW、CCW_STOP,CW_STOP
    Tick    = 0,               -- 当前状态下累加计时，>0: 电机转动计时, <0: 停机计时

    GapTick     = 1000,       -- 正反转切换时间tick_5ms = 3秒

    Alarm       =""             --告警信息
    --tick_50ms_A =0,         --启动时的tick
    --Dip_A =0                --启动时的倾角
}

------------------- 局部变量  ----------------------------
local   period = 50       -- 50ms的时间片
local   t = 0             -- 记录电机运行时间t，计算speed用
local   d1 = 0            -- 电机启动时刻的角度，电机速度speed =(结束的角度-d1)/t



--------------------------------------------------------------
-- 电机正,反转,停机运行的的状态机
--------------------------------------------------------------

sys.timerLoopStart(function()	

        --正转状态
        if Motor.Status == 'CW' then
            Motor.Tick = Motor.Tick + period  -- 电机转动时间累加
            Motor.Run =  Motor.Run - period     -- 驱动控制计时减一
            
            if (Motor.Run> 0) and LimitSwitchCW_On() == true then
                MOTOR_CW()
                Motor.Alarm = ""
            else                                        --驱动计时=0
                if not LimitSwitchCW_On() then
                    Motor.Alarm = "LimitSwitchCW_Error"
                end

                if Motor.Run > -period*2 then       -- 如果接近0点,直接清零
                    Motor.Run =0
                end
                Motor.Status = 'CW_STOP'                --进入停机状态
                --t = Motor.Tick                       -- 记录电机运行时间t
                Motor.Tick =0                        -- 电机转动时间（CWSTOP停机）=0
                MOTOR_STOP()                            -- 发送停机信号
            end
        

        --反转状态
        elseif Motor.Status == 'CCW' then
            Motor.Tick = Motor.Tick + period      -- 电机转动时间累加
            Motor.Run =  Motor.Run + period         -- Motor.Run<0: 反转 
            if (Motor.Run <0)  and LimitSwitchCCW_On() == true then            
                MOTOR_CCW()
                Motor.Alarm = ""
            else
                if not LimitSwitchCCW_On() then
                    Motor.Alarm = "LimitSwitchCCW_Error"
                end
                
                if Motor.Run < period*2 then        -- 如果接近0点,直接清零
                    Motor.Run =0
                end
                Motor.Status = 'CCW_STOP'
                t = Motor.Tick                           -- 记录电机运行时间t
                Motor.Tick =0
                MOTOR_STOP()
            end
        

        -- 正转停机
        elseif Motor.Status == 'CW_STOP' then
            MOTOR_STOP()                        
            Motor.Tick = Motor.Tick - period      -- 电机停机时间累减

            --[[
            -- 在某时刻（3秒钟），计算电机转速
            if Motor.Tick <= -Motor.GapTick and Motor.Tick > - (Motor.GapTick + period) and d1 ~=0 then 
                Motor.CW_Speed = (Inclinator.x - d1)/t *1000     
                log.debug("Motor", "CW_Speed = (d2("..Inclinator.x..")-d1("..d1.."))/t("..t..")="..Motor.CW_Speed )
            end

            --电机启动，记录启动时的角度d1
            if (Motor.Run ~=0) then
                if (Motor.Tick < -Motor.GapTick)  then     -- 停机时间超过时间间隔,启动电机
                    d1 = Inclinator.x      --记录启动的时间和角度
                else
                    d1 = 0           --停止时间不足,清零
                end
            end 
            ]]   

            if Motor.Run > period*2 then         --RunTick>0,启动正转
                Motor.Status = 'CW'
                Motor.Tick =0     
            end
            
            if (Motor.Run< -period*2) and (Motor.Tick <= -Motor.GapTick)  then     -- 停机时间超过时间间隔,启动反转
                Motor.Status = 'CCW'
                Motor.Tick =0
            end     

        -- 反转停机
        elseif Motor.Status == 'CCW_STOP' then
            MOTOR_STOP()
            Motor.Tick = Motor.Tick - period      -- 电机停机时间累减
            
            if Motor.Tick < -Motor.GapTick and Motor.Tick >= -(Motor.GapTick+ period) and d1~=0 then 
                Motor.CCW_Speed = (Inclinator.x - d1)/t *1000        -- 在一定时刻，计算电机转速
                log.debug("Motor", "CCW_Speed = (d2("..Inclinator.x..")-d1("..d1.."))/t("..t..")="..Motor.CCW_Speed )
            end
            --[[
            ----电机启动，记录启动时的角度d1
            if (Motor.Run ~=0) then
                if (Motor.Tick < -Motor.GapTick)  then     -- 停机时间超过时间间隔,启动电机
                    d1 = Inclinator.x      --记录启动的时间和角度
                else
                    d1 = 0           --停止时间不足,清零
                end
            end 
            ]]    

            -- 转换状态
            if Motor.Run > period*2 and (Motor.Tick <= -Motor.GapTick) then         --RunTick>0,启动正转
                Motor.Status = 'CW'
                Motor.Tick =0     
            elseif (Motor.Run < -period*2)  then     -- 停机时间超过时间间隔,启动反转
                Motor.Status = 'CCW'
                Motor.Tick =0
            end    
        end

end, period)





